// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MockFloorFilter.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Runtime
{
    using System;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Runtime.Cameras;    

    /// <summary>
    /// Mock Correction utility class for injecting into Kinect processor 
    /// </summary>
    public partial class MockFloorFilter : IKinectFloorFilter
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="MockFloorFilter" /> class.
        /// </summary>
        public MockFloorFilter()
        {
        }

        /// <summary>
        /// Set floor filtering data
        /// </summary>
        /// <param name="floorFilteringStripConstants">An array initialized with pre-learned A and B coefficients in rows, per strip of floor (columns)</param>
        public void SetCalibrationData(double[][] floorFilteringStripConstants)
        {
        }

        /// <summary>
        /// Is initialized
        /// </summary>
        /// <returns>Self explanatory</returns>
        public bool IsInitialized()
        {
            return true;
        }

        /// <summary>
        /// Update Floor Plane Values
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="frameHeight">Frame height</param>
        /// <returns>Whether or not floor was learned from the frame</returns>        
        public bool TryLearnFloor(short[] depthFrame, int frameWidth, int frameHeight)
        {
            return true;
        }

        /// <summary>
        /// Applies a linear correction to a depth reading. Right now, this supports 1st and second degree polynomials
        /// It probably makes sense to optimize for one and remove the other once we have enough trust in one of them
        /// </summary>
        /// <param name="depth">Depth reading to convert</param>
        /// <param name="row">The row</param>
        /// <param name="col">The column</param>
        /// <returns>Corrected depth reading</returns>
        public short FilterPlanePixels(short depth, int row, int col)
        {
            if (depth > 2000 && depth < 3000)
            {
                return DepthImageConstants.FloorValue;
            }
            else if (depth > 6000 && depth < 7000)
            {
                return DepthImageConstants.CeilingValue;
            }

            return depth;
        }

        /// <summary>
        /// Generate coefficients for floor extraction. 
        /// </summary>
        /// <param name="profile">Strip profile</param>
        /// <param name="rows">Row count</param>
        /// <param name="startX">Start X</param>
        /// <param name="seedA">best known A</param>
        /// <param name="maxIterations">Max iterations</param>
        /// <param name="smallestAStep">Smallest A step after which search will be stopped</param>
        /// <param name="initialAStep">Initial A step</param>
        /// <returns>A, B and variance of computed constants for this strip of floor</returns>
        public Tuple<double, double, double> FindFittingConstantsForFloorStrip(double[] profile, int rows, int startX, double seedA, int maxIterations, double smallestAStep, double initialAStep)
        {
            return null;
        }

        /// <summary>
        /// Compute strip averages
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="startRow">Start row</param>
        /// <param name="endRow">End row</param>
        /// <param name="startCol">Start column</param>
        /// <param name="endCol">End column</param>
        /// <param name="averageStripDepth">Out average strip depth</param>
        /// <returns>Strip average depths</returns>
        public double[] GetStripAverages(
            short[] depthFrame,
            int frameWidth,
            int startRow,
            int endRow,
            int startCol,
            int endCol,
            out double averageStripDepth)
        {
            averageStripDepth = 0;
            return null;
        }

        /// <summary>
        /// Generate coefficients for floor extraction. 
        /// Function we are trying to approximate has a form Y = B / (X - A)
        /// </summary>
        /// <param name="profile">Strip profile</param>
        /// <param name="stripRows">Row count</param>
        /// <param name="startX">Start X</param>
        /// <param name="seedA">best known A</param>
        /// <param name="maxIterations">Max iterations</param>
        /// <param name="smallestAStep">Smallest A step after which search will be stopped</param>
        /// <param name="initialAStep">Initial A step</param>
        /// <param name="outResult">Out result best A and B constants and Variance that we get with those constants against sample data</param>
        /// <returns>Whether or not fitting was at all successful</returns>
        public bool FindFittingConstantsForStrip(
            double[] profile,
            int stripRows,
            int startX,
            double seedA,
            int maxIterations,
            double smallestAStep,
            double initialAStep,
            out FastTuple<double, double, double> outResult)
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Walk through entire array and filter our floor 
        /// </summary>
        /// <param name="depthFrame">Depth frame</param>
        /// <param name="frameWidth">Frame width</param>
        /// <param name="frameHeight">Frame height</param>
        public void FilterPlanesOnFrame(short[] depthFrame, int frameWidth, int frameHeight)
        {
            for (int row = 0; row < frameHeight; ++row)
            {
                int indexRowTemp = row * frameWidth;

                for (int col = 0; col < frameWidth; ++col)
                {
                    int pixelIndex = indexRowTemp + col;

                    short depthVal = depthFrame[pixelIndex];

                    if (depthVal > 0)
                    {
                        double correctedDepth = this.FilterPlanePixels(depthVal, row, col);

                        depthFrame[pixelIndex] = (short)Math.Round(correctedDepth);
                    }
                }
            }
        }
    }
}
